#ifndef PULL_ENCODER_HPP_
#define PULL_ENCODER_HPP_

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <errno.h>
#include <string.h>
#include <wiringPi.h>
#include <memory>

#define PINA 4
#define PINB 5

class PullEncoder
{
private:
    static uint8_t state;
    static long pos;
public:
    PullEncoder(/* args */);
    ~PullEncoder();
    int get_pos();
    static void pin_isr(void);
};

PullEncoder::PullEncoder(/* args */)
{
    if (wiringPiSetup() < 0) {
    //RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiSetup");
    exit(EXIT_FAILURE);
    }

    pinMode (PINA,  INPUT) ;
    pinMode (PINB,  INPUT) ;
    pullUpDnControl(PINA, PUD_UP);
    pullUpDnControl(PINB, PUD_UP);

    //RCLCPP_INFO(rclcpp::get_logger("ROTARY_ENCODER", "Encoder init success.");

    if ( wiringPiISR (PINA, INT_EDGE_BOTH, &pin_isr) < 0 ) {
       // RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiISR");
        exit(EXIT_FAILURE);
    }
    
    if ( wiringPiISR (PINB, INT_EDGE_BOTH, &pin_isr) < 0 ) {
       // RCLCPP_FATAL(rclcpp::get_logger("ROTARY_ENCODER"), "wiringPiISR");
        exit(EXIT_FAILURE);
    }
}

PullEncoder::~PullEncoder()
{
}

void PullEncoder::pin_isr(void)
{
    uint8_t p1val = digitalRead(PINA);
    uint8_t p2val = digitalRead(PINB);
    uint8_t s = state & 3;
    if (p1val) s |= 4;
    if (p2val) s |= 8;
    state = (s >> 2);
    
    switch (s) {
        case 1: case 7: case 8: case 14:
            pos++;
            return;
        case 2: case 4: case 11: case 13:
            pos--;
            return;
        case 3: case 12:
            pos += 2;
            return;
        case 6: case 9:
            pos -= 2;
            return;
    }
}

int PullEncoder::get_pos()
{
    return this->pos;
}


long PullEncoder::pos;
uint8_t PullEncoder::state;

#endif
